#ifndef __MC_MATH_H
#define __MC_MATH_H

#include "stdint-gcc.h"

#define LOG2(x) \
  ((x) == 65535 ? 16 : \
   ((x) == 2*2*2*2*2*2*2*2*2*2*2*2*2*2*2 ? 15 : \
    ((x) == 2*2*2*2*2*2*2*2*2*2*2*2*2*2 ? 14 : \
     ((x) == 2*2*2*2*2*2*2*2*2*2*2*2*2 ? 13 : \
      ((x) == 2*2*2*2*2*2*2*2*2*2*2*2 ? 12 : \
       ((x) == 2*2*2*2*2*2*2*2*2*2*2 ? 11 : \
        ((x) == 2*2*2*2*2*2*2*2*2*2 ? 10 : \
         ((x) == 2*2*2*2*2*2*2*2*2 ? 9 : \
          ((x) == 2*2*2*2*2*2*2*2 ? 8 : \
           ((x) == 2*2*2*2*2*2*2 ? 7 : \
            ((x) == 2*2*2*2*2*2 ? 6 : \
             ((x) == 2*2*2*2*2 ? 5 : \
              ((x) == 2*2*2*2 ? 4 : \
               ((x) == 2*2*2 ? 3 : \
                ((x) == 2*2 ? 2 : \
                 ((x) == 2 ? 1 : \
                  ((x) == 1 ? 0 : -1)))))))))))))))))




#define INT_ABS(a) ((a > 0) ? a : (-a))


#define SIN_MASK        0x0300u
#define U0_90           0x0200u
#define U90_180         0x0300u
#define U180_270        0x0000u
#define U270_360        0x0100u

#define DIV_SQRT_3      (int32_t)0x49E6 /* 1/sqrt(3) in q1.15 format=0.5773315*/
#define SQRT_3_DIV2     (int32_t)0x6ED8 //sqrt(3)/2
#define DIV_1_2         (int32_t)0x4000 //1/2
#define SQRT_3          (int32_t)0xDDB0 //sqrt(3)

#define SPEED_ARR_CNT   3  //cal speed every 1ms and update average spd CNT ms
#define CIRCLE_ENC_CNT  16384 //get 16384 cnt every mechanical circle
typedef struct
{
  int16_t hcos; //cos value
  int16_t hsin; //sin value
} TrigComponents;

typedef struct
{
    int16_t alpha;
    int16_t beta;
}AlphaBeta_t;

typedef struct 
{
  int16_t d;//park output d
  int16_t q;//prak output q
}Qd_t;

typedef struct
{
  int16_t a;//clark input a
  int16_t b;//clark input b
} Ab_t;

typedef struct 
{
  int32_t u;//unti clark out u
  int32_t v;//unti clark out v
  int32_t w;//unti clark out w

  int32_t ta;
  int32_t tb;
  int32_t tc;
}Uvw_t;

TrigComponents trig_functions(int16_t hangle);
AlphaBeta_t rev_park_conv(Qd_t input, int16_t theta);
AlphaBeta_t clarke_conv(Ab_t input);
Qd_t park_conv(AlphaBeta_t input, int16_t theta);
void rev_clarke_conv(AlphaBeta_t input, Uvw_t *uvw);
uint8_t sector_judgment(AlphaBeta_t input);
int8_t pwm_duty_out(AlphaBeta_t input, uint16_t tim_arr, uint8_t sector, Uvw_t* uvw);
int16_t low_pass_filter(int16_t input, int16_t last_out, uint16_t coe);

#endif